#include <iostream>
#include <pthread.h>

#include "Sys3D.h"
#include "Quaternion.h"
#include "Rotation.h"
#include "MatrixClasses.h"
#include "FastOps.h"
#include "LU.h"

#include "Timer.h"

using namespace RSIM;

int main (int argc, const char * argv[])
{        
        const double& q = 1.0;
        const double& u = 1.0;
        
        const int& nbodies = 512;
        const int& nproc = 1;
        const int& niter = 10000;
        
        RSIM::Sys3D mbs3D(nbodies,nproc);
        mbs3D.initDCANodesForDCAABA();
        mbs3D.setQ(q);
        mbs3D.setU(u);        
        mbs3D.updRotationMatrix();
        mbs3D.updAngularVelocity();
//         mbs3D.calcJ();
        
//         mbs3D.n_formInboardEOM(0,nbodies-1);
        /*mbs3D.n_calcDCAHandleEquationsUsingLillyOrin_Opt(0,3,0);
        mbs3D.n_calcDCAHandleEquationsUsingABA_Opt(4,nbodies-1,1,true,true);
        DCAAssembly3D *Asm = mbs3D.Asm_;
        mbs3D.n_assemble_Opt(Asm[0],Asm[1],Asm[2],true,false);
        
        Asm[2].dca_.displayH1();
        Asm[2].dca_.displayH2()*/;
        
        Timer t;
        for(int i=0;i<niter;++i){
                mbs3D.n_formInboardEOM(0,nbodies-1);        
                mbs3D.n_calcDCAHandleEquationsUsingLillyOrin_Opt(0,nbodies-1,0);
        }                
        
        t.stop();                  
        t.printElapsedTime(0);
        
        // cout<<mbs3D.barray_[nbodies-1].Dat_.Lambda<<endl<<endl;
        
//         Matrix I(6,6);
//         I.Identity();
//         Matrix M;
//         LU lu(mbs3D.barray_[nbodies-1].Dat_.Lambda);
//         M = lu.solve(I);
//         cout<<M<<endl;
// //         
//         cout<<M*mbs3D.barray_[nbodies-1].Dat_.p<<endl;
                
        return 0;
}
